#pragma once
#include <cv.h>
#include <highgui.h>
#include <cxcore.h>
#include <vector>
#include "Structures.h"

using namespace std;

namespace Stereo
{



#define HOMOGENEOUS		0	//homogenous reconstruction
#define TRIANGULATION	1	//triangulation reconstruction not implemented yet

//#define DEBUG_FUNDAMENTAL_MATRIX	//debug fundamental matrix


//#define ESTIMATE_ESSENTIAL_DIRECTLY

class Point3dReconstructor
{
public : 
	Point3dReconstructor();
	~Point3dReconstructor();
	void Reconstruct3dPointHomogeous(
							const CvPoint2D32f & Left2DPoint,
							const CvPoint2D32f & Right2DPoint, 
							CvPoint3D32f & point3d,
							CvMat *LeftProjectionMatrix,
							CvMat * RightProjectionMatrix
							);
	int Reconstruct3dpointTriangulation(
		const CvPoint2D32f & Left2DPoint,
		const CvPoint2D32f & Right2DPoint,
		CvPoint3D32f & leftpoint3d,
		CvPoint3D32f & rightpoint3d,
		const CvMat *leftintrinsic,
		const CvMat *rightintrinsic,
		const CvMat *RotationMatrix,
		const CvMat *Translationvector
		);
	int Reconstruct3dpointTriangulation(
		const CvPoint2D32f & Left2DPoint,
		const CvPoint2D32f & Right2DPoint,
		CvPoint3D32f & leftpoint3d,
		CvPoint3D32f & rightpoint3d,
		const CameraParameters& left_CameraParameters,
		const CameraParameters& right_CameraParameters,
		const CvMat *RotationMatrix,
		const CvMat *Translationvector);
	
private:
	void InitHomogenous();
	void InitTriangulation();
	CvMat *P3DREC_W;
	CvMat *P3DREC_V;
	CvMat *P3DREC_U;
	CvMat *P3DREC_A;
	CvMat *P3DREC_X;
	//triangulation attributes
	CvMat *P3DREC_A_;
	CvMat *P3DREC_A1;
	CvMat *P3DREC_invAtA;
	CvMat *P3DREC_invAtAAt;
	CvMat *P3DREC_res_Z;

};
class StereoMatcher
{

public : 
		StereoMatcher();


		~StereoMatcher();
		int Analyse();
		void SetCamerasParameters(const CameraParameters &,const CameraParameters &);
		void Load2dPoints(const  std::vector<CvPoint > & left_points,const std::vector<CvPoint > & right_points);
		void DrawCorrespondingEpilines(IplImage *left_images,IplImage *right_images);
		int GetPointsNumber();
		void GetGoodRightProjectionMatrix(CvMat *Rmat,CvMat *R,CvMat *T,CvMat *Essential,CvMat *Fundamental);
		void ReconstructTrinagulation3DPointWithGoodProjectionMatrixTrinagulation(
														const CvPoint2D32f & Left2DPoint,
														const CvPoint2D32f & Right2DPoint,
														CvPoint3D32f & leftpoint3d,
														CvPoint3D32f & rightpoint3d
														);

private:

		int FindFundamentalMatrix();
		void ValidateFundamentalMatrix();
		
		int FindEssentialMatrix();
		void DecomposeEssentialMatrix();

		void ConstructTheFourProjectionMatrix();
		void SelectGoodRightProjectionMatrix();

		//attributes
		//2d points containers
		std::vector<CvPoint2D32f > sm_points[2];//left camera points = sm_points[0]
		CvMat sm_points_left;
		CvMat sm_points_right;
		
		//camera parameters
		std::vector<CvMat *>sm_camerasparameters;
		std::vector<CvMat *>sm_camerasdistortcoeffs;
		
		//number of points used for cvfindfundamentalmatrix
		int sm_numpoints;
		
	
		//fundamental and essential matrix
		
		CvMat* sm_fundMatr;
		CvMat* sm_essentialMatr;

		//status returned from cvfindfundamentalmatrix
		CvMat* sm_status;

		//epipolar lines
		std::vector<CvPoint3D32f> sm_lines[2];

		//extracted from the svd of E 
		CvMat *sm_Ra;
		CvMat *sm_Rb;
		CvMat *sm_T;
		
		//four possible reconstruction for right matrix matrix supposing that the left matrix has a projection matrix K [I | 0]
		CvMat *sm_PR4[4];
		//projection matrix for left camera where PL=K*I; where I is the identity matrix
		CvMat *sm_PL;
		

		//the final R and T 
		bool sm_found_good_matrix;
		CvMat *sm_good_R;
		CvMat *sm_good_RotMatrix;
		CvMat *sm_good_T;
		//3d reconstrcutor or solver 
		Stereo::Point3dReconstructor *sm_point3dreconstructor;
};

class POSIT  
{

public:
	POSIT();
	POSIT(Dimension dimension,CameraParameters camparam,int nbpoints,std::vector<CvPoint3D32f > points);
	virtual ~POSIT();

	void SetObjectDimension(Dimension dimension);
	void InitializeIntrinsics( CameraParameters camparam  );
	void EstimateObjectPos();

	void GetImagePoints(IplImage *image);
	void GetObjetPoints(int nbpoints,std::vector<CvPoint3D32f > points);
	CvMat *GetHomogeneousMatrix();
	double *GetOpenGLMatrix();

private:
	
	
	//CvMat *po_intrinsics;
	std::vector<CvMat *> po_intrinsics;
	std::vector<CvMat *> po_extrinsics;
	//matrice opengl
	double po_posePOSIT[16];

	int po_nbpoints;
	Dimension po_objectdimension;
	vector<CvPoint3D32f> po_modelPoints;
	vector<CvPoint2D32f> po_imagePoints;
	CvPOSITObject* po_positObject;

	CvMatr32f po_rotation_matrix;
	CvVect32f po_translation_vector;
protected:
	void CreateOpenGLMatrix(const CvMatr32f &rotationMatrix, const CvVect32f &translationVector);
	void CreateMatrix(const CvMatr32f &rotationMatrix, const CvVect32f &translationVector);
};


class CameraCalibrator
{
public :
	CameraCalibrator();
	~CameraCalibrator();
	int CalibrateCamera(const std::vector<IplImage *> images,const CvSize& grid_dim,const int winx=11,int const winy=11,const float squaresize=80.0f,CameraParameters &p=CameraParameters());
	std::vector<IplImage *> BuildUndistortionMap(const CameraParameters &c,const float imagewidth,const float imageheight) const ;
	IplImage * UndistortImage(const IplImage *image,const std::vector<IplImage *> map) const ;
private :

	int CC_image_width;
	int CC_image_height;
	CvSize CC_board_size;
	
	int CC_points_in_board;
	int CC_nb_boards;
};

class StereoCalibrator
{
	public : 
		StereoCalibrator();
		~StereoCalibrator();
		int  CalibrateStereoCameras(
			const std::vector<IplImage *> left_images,
			const std::vector<IplImage *> right_images,
			CameraParameters &left_camera_parameters,
			CameraParameters &right_camera_parameters,
			CvMat* translationvector,
			CvMat* rotationmatrix,
			CvMat* rotationvec,
			const CvSize& grid_dim,
			CvMat* essential,
			CvMat*fundamental,
			const int winx=11,
			const int winy=11,
			const float squaresize=30.0f,
			int fixintrinsic=1
			);
private :
	int SC_image_width;
	int SC_image_height;
	CvSize SC_board_size;
	int SC_points_in_board;
	int SC_nb_boards;
};
};